use std::{sync::Arc, thread::sleep, time::Duration};

use realman::traits::{ModbusTrait, MotionTrait};

use crate::server::AppState;

pub async fn to_save_go_home(app_state: Arc<AppState>, goto: [i32; 7]) {
    if let Ok(mut arm) = app_state.arm_listen.lock() {
        // begin
        arm.movej(
            realman::ArmType::Gen72([0, 50000, 0, -54000, 0, 18000, 725]),
            20,
            0,
            realman::TrajectoryConnect::Now,
        )
        .ok();
        arm.write_single_register(realman::ModbusPortType::EndRS485RTUMain, 40000, 100, 1)
            .ok();
        sleep(Duration::from_secs(1));
        arm.movej(
            realman::ArmType::Gen72(goto),
            20,
            0,
            realman::TrajectoryConnect::Now,
        )
        .ok();
        arm.write_single_register(realman::ModbusPortType::EndRS485RTUMain, 40000, 0, 1)
            .ok();
        sleep(Duration::from_secs(1));
        arm.movej(
            realman::ArmType::Gen72([0; 7]),
            20,
            0,
            realman::TrajectoryConnect::Now,
        )
        .ok();

        // arm.movej(
        //     realman::ArmType::Gen72([-157558, -33329, -15922, 0, 0, 0, 0]),
        //     20,
        //     0,
        //     realman::TrajectoryConnect::Now,
        // )
        // .ok();
        arm.movej(
            realman::ArmType::Gen72([-158777, -25322, -12713, -119379, -8442, 6938, 20869]),
            20,
            0,
            realman::TrajectoryConnect::Now,
        )
        .ok();
        arm.write_single_register(realman::ModbusPortType::EndRS485RTUMain, 40000, 100, 1)
            .ok();
        sleep(Duration::from_secs(1));
        // arm.movej(
        //     realman::ArmType::Gen72([-157558, -33329, -15922, 0, 0, 0, 0]),
        //     50,
        //     0,
        //     realman::TrajectoryConnect::Now,
        // )
        // .ok();
        arm.movej(
            realman::ArmType::Gen72([0; 7]),
            20,
            0,
            realman::TrajectoryConnect::Now,
        )
        .ok();
        arm.write_single_register(realman::ModbusPortType::EndRS485RTUMain, 40000, 0, 1)
            .ok();
    }
}

// 点头
pub async fn head_point(app_state: Arc<AppState>) {
    if let Ok(mut arm) = app_state.arm_listen.lock() {
        arm.movej(
            realman::ArmType::Gen72([0; 7]),
            50,
            0,
            realman::TrajectoryConnect::Now,
        )
        .ok();
        for _ in 0..3 {
            arm.set_joint_step(&[6, -15000], 100).ok();
            arm.set_joint_step(&[6, 15000], 100).ok();
        }
    }
}
